/* Copyright (C) 2010 Fredrique Samuels, fredriquesamuels@gmail.com 

 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "matrix.h"

namespace tri
{

std::ostream &operator<<( std::ostream& out, const matrix& m){
    out<<"<Matrix: ";
    for (int i = 0;i<16;i++){
        out<<" "<<m.elements[i];
    }
    out<<" >"<<std::endl;
    return out;
}

matrix::matrix()
{
    load_identity(this);
}

matrix::~matrix()
{
}

void matrix::load_identity(tri::matrix* m){
    for (int i = 0;i<16;i++){
        m->elements[i] = 0;
        if( i == 0 ||
            i % 5 == 0){
            m->elements[i] = 1.0f;
        }
    }
}

void matrix::translate(matrix* m,float x,float y,float z){
    m->elements[12]+=x;
    m->elements[13]+=y;
    m->elements[14]+=z;
}

void matrix::get_rotation_matrixX(float rot,matrix*m){
    load_identity(m);
    float d = (M_PI/180.0f) * rot;
    m->elements[6] = cos(d);
    m->elements[7] = -sin(d);
    m->elements[9] = sin(d);
    m->elements[10] = cos(d);
}

void matrix::get_rotation_matrixY(float rot,matrix*m){    
    load_identity(m);
    float d = (M_PI/180.0f) * rot;
    m->elements[0] = cos(d);
    m->elements[5] = sin(d);
    m->elements[8] = -sin(d);
    m->elements[10] = cos(d);
}

void matrix::get_rotation_matrixZ(float rot,matrix*m){
    load_identity(m);
    float d = (M_PI/180.0f) * rot;
    m->elements[0] = cos(d);
    m->elements[1] = -sin(d);
    m->elements[4] = sin(d);
    m->elements[5] = cos(d);
}

void matrix::get_rotation_matrixXYZ(float x,float y,float z,matrix*m){
    
    matrix mx,my,mz,mxy;
    get_rotation_matrixX(x,&mx);
    get_rotation_matrixY(y,&my);
    get_rotation_matrixZ(z,&mz);
    
    mult_mat(&mx,&my,&mxy);
    mult_mat(&mxy,&mz,m);
    
}

//static void get_rotation_matrixXYZ(float rot,matrix*m);

void matrix::mult_mat(const matrix*m1,const matrix*m2,matrix*res){
    load_identity(res);
    for(int i = 0;i<4;i++){
        for(int j = 0;j<4;j++){
        res->elements[i*4+j] = m1->elements[i*4+0]*m2->elements[0+j] + 
                               m1->elements[i*4+1]*m2->elements[4+j] + 
                               m1->elements[i*4+2]*m2->elements[8+j] + 
                               m1->elements[i*4+3]*m2->elements[12+j];
        }
    }
}


void matrix::mult_vmat(const matrix* m,const vector* v,vector* dst){
    
    matrix rot,res;
    translate(&rot,v->x,v->y,v->z);
    mult_mat(&rot,m,&res);
    dst->x = res.elements[12];
    dst->y = res.elements[13];
    dst->z = res.elements[14];
    
}

}
